/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_low
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

#ifndef ROSPKG_SCHUNK_LOW_SCHUNK_LOW_CONTROL_H_
#define ROSPKG_SCHUNK_LOW_SCHUNK_LOW_CONTROL_H_

/******************************************************************************
                                   INCLUDES
 *****************************************************************************/

// schunk_low
#include <schunk_low/schunk.h>

// General
#include <vector>

// ROS
#include <ros/callback_queue.h>

// Messages
#include <schunk_msgs/JointPositions.h>
#include <schunk_msgs/JointVelocities.h>

// Service Servers
#include <schunk_low/Init.h>
#include <schunk_low/Home.h>
#include <schunk_low/Stop.h>
#include <schunk_low/Recover.h>
#include <schunk_low/GripperOpen.h>
#include <schunk_low/GripperClose.h>
#include <schunk_low/GripperSetVel.h>
#include <schunk_low/Connect.h>
#include <schunk_low/Disconnect.h>
#include <schunk_low/ControlMode.h>
#include <schunk_low/GetPosVel.h>

/******************************************************************************
                            CLASS SCHUNK_LOW_CONTROL
 *****************************************************************************/

// Enumerations
enum ControlMode {MIN_CONTROL_MODE = 0, CONTROL_MODE_VELOCITY = 1, CONTROL_MODE_POSITION = 2, MAX_CONTROL_MODE = 3};

class SchunkLowControl
{
    private:

        // Node Handlers
        ros::NodeHandle service_server_nodehandle_;
        ros::NodeHandle velocity_nodehandle_;
        ros::NodeHandle position_nodehandle_;
        ros::NodeHandle publisher_nodehandle_;
        ros::NodeHandle schunk_nodehandle_;

        // Publishers
        ros::Publisher publisher_parameters_;
        ros::Publisher publisher_simple_;
        ros::Publisher publisher_complete_;
        ros::Publisher publisher_electrical_;

        // Subscribers
        ros::Subscriber subscriber_velocity_;
        ros::Subscriber subscriber_position_;

        // Service Servers
        ros::ServiceServer service_server_init_;
        ros::ServiceServer service_server_home_;
        ros::ServiceServer service_server_stop_;
        ros::ServiceServer service_server_recover_;
        ros::ServiceServer service_server_gripper_open_;
        ros::ServiceServer service_server_gripper_close_;
        ros::ServiceServer service_server_gripper_set_vel_;
        ros::ServiceServer service_server_connect_;
        ros::ServiceServer service_server_disconnect_;
        ros::ServiceServer service_server_control_mode_;
        ros::ServiceServer service_server_get_pos_vel_;

        // Subscribed Topic Callbacks
        void VelocityCallback(const schunk_msgs::JointVelocities::ConstPtr& msg);
        void PositionCallback(const schunk_msgs::JointPositions::ConstPtr& msg);

        // Services Server Callbacks
        bool InitCallback(schunk_low::Init::Request& req, schunk_low::Init::Response& resp);
        bool HomeCallback(schunk_low::Home::Request& req, schunk_low::Home::Response& resp);
        bool StopCallback(schunk_low::Stop::Request& req, schunk_low::Stop::Response& resp);
        bool RecoverCallback(schunk_low::Recover::Request& req, schunk_low::Recover::Response& resp);

        bool GripperOpenCallback(schunk_low::GripperOpen::Request& req, schunk_low::GripperOpen::Response& resp);
        bool GripperCloseCallback(schunk_low::GripperClose::Request& req, schunk_low::GripperClose::Response& resp);
        bool GripperSetVelCallback(schunk_low::GripperSetVel::Request& req, schunk_low::GripperSetVel::Response& resp);

        bool ConnectCallback(schunk_low::Connect::Request& req, schunk_low::Connect::Response& resp);
        bool DisconnectCallback(schunk_low::Disconnect::Request& req, schunk_low::Disconnect::Response& resp);
        bool SetControlModeCallback(schunk_low::ControlMode::Request& req, schunk_low::ControlMode::Response& resp);
        bool GetPosVelCallback(schunk_low::GetPosVel::Request& req, schunk_low::GetPosVel::Response& resp);

    public:

        // Node Handlers
        ros::NodeHandle nodehandle_;

        // Callback Queues
        ros::CallbackQueue service_server_queue_;
        ros::CallbackQueue velocity_queue_;
        ros::CallbackQueue position_queue_;
        ros::CallbackQueue publisher_queue_;

        // Robot object
        Schunk* schunk_;

        // Robot auxiliar variables
        std::vector<double> max_pos_;
        std::vector<double> min_pos_;
        std::vector<double> std_vel_;
        std::vector<double> std_acc_;

        // Loop variables
        int frequency_;
        ControlMode control_mode_;
        ros::Time last_publish_time_;
        ros::Duration min_publish_duration_;

        // Status variables
        bool initialized_;
        bool connected_;
        bool has_gripper_;
        int connection_NID_;

        // Constructor
        SchunkLowControl();

        // Destructor
        ~SchunkLowControl();

        // Parameter loading functions
        bool GetROSParameters();
        bool GetRobotDescriptionParameters();
        bool GetPIDParameters();

        // Publish functions
        void PublishState();

        // Auxiliar functions
        bool CheckJointLimits(const std::vector<double>& cur_pos, const std::vector<double>& next_vel, std::vector<double> *limited_vel);
};

#endif  // ROSPKG_SCHUNK_LOW_SCHUNK_LOW_CONTROL_H_
